#include <vector>
#include <GL/glew.h>
#include "CudaSPH.h"
#include <glm/glm.hpp>
#include <GL/glfw.h>
#include "SimulationManager.h"
#include <cmath>


#define WINDOW_W 600
#define WINDOW_H 600
#define DIMENSIONALITY 2
#define GRID_MIN_PT 0
#define GRID_MAX_PT 12

// Physical parameters
#define TIME_STEP 0.004f
#define SUPPORT_RAD 0.3f // was 1.0f for 3d
#define GAS_CONSTANT_K 1.1f//water
#define VISCOSITY_CONSTANT 0.4f
#define GAMMA 7.0f

float particleMass = 1.0;
std::vector<int> testGridNdx;
std::vector<CudaParticle> testParticles;
std::vector<float> testForces;
std::vector<int> testGridEnd;
std::vector<float> testPress;
std::vector<unsigned int> testOctree;

// Globals
CudaSPH * cudaSim;
bool doDrawGrid = true;

// Functions protos

void IntegrateParticlesCUDA();


//A global simulation manager.
//This takes care of most boiler-plate code.
//If you only want to draw something, you only need to do three things:
//1) Create a display func with the function signerature
//	void funcname()
//2) In main, call 
//	sm.SetCallbackDisplay(funcname)
//  where funcname is the name of your draw function.
//3) In main, after the above function call, call
//	sm.Run();
//  This will start running the simulation.
// 
//  For a list of the callbacks the user can set, look at the "SetCallback..." functions 
//  in the SimulationManager header file.
//  Also, the GetCamera() method will get the camera and allow you to call methods which move
//	around the scene or rotate, etc.
SimulationManager sm(WINDOW_W, WINDOW_H);

void DrawPoints() {

	glPointSize(3.0f);
	glColor3f(0.0,1.0,0.0);
	glBegin(GL_POINTS);
	for (int i = 0; i < testParticles.size(); ++i){
		glVertex3f(testParticles[i].pos.x, testParticles[i].pos.y, testParticles[i].pos.z);
	}
	glEnd();
}

void DrawGrid(){
	float scellw = 2*SUPPORT_RAD;
	int snumcells = std::ceil((GRID_MAX_PT - GRID_MIN_PT)/(scellw));

	glm::vec3 cellW = glm::vec3(scellw, scellw, scellw);
	glm::vec3 numCells = glm::vec3(snumcells, snumcells, snumcells);

	glPolygonMode(GL_FRONT, GL_LINE);
	glBegin(GL_QUADS);
	for (int i = 0; i < testGridEnd.size(); ++i){
		if(testGridEnd[i] == -1) { continue; }
		glm::vec3 lc = 
			glm::vec3( (i % snumcells)*scellw,
		      (i / snumcells)*scellw,
				0 );

		glVertex3f(lc.x, lc.y, lc.z);
		glVertex3f(lc.x+scellw, lc.y, lc.z);
		glVertex3f(lc.x+scellw, lc.y+scellw, lc.z);
		glVertex3f(lc.x, lc.y+scellw, lc.z);
	}
	glEnd();
	glPolygonMode(GL_FRONT, GL_FILL);
}

float GetOctreeCellW( int currLev, int numLev ) {
	int rel = numLev - currLev;
	
	int power = pow(2.0f, rel);

	return (2*SUPPORT_RAD)*power;
}

void GetOctreeLowerC( unsigned int oNdx, unsigned int numLev, float * outLowerC ) {
	outLowerC[0] = GRID_MIN_PT; outLowerC[1] = GRID_MIN_PT; outLowerC[2] = GRID_MIN_PT;

	int numBits = (numLev-1)*3;
	numBits += numBits%3;

	for (int i = numLev; i > 1; --i){

		float cellW = GetOctreeCellW(i, numLev);
		printf("cellW: %f\n", cellW);
		outLowerC[0] += (oNdx & 0x01)*cellW;
		outLowerC[1] += (oNdx & 0x02)*cellW;
		outLowerC[2] += (oNdx & 0x04)*cellW;

		oNdx = oNdx >> 3;

	}
}

int GetSNdx( int lev ){
	if (lev == 1){
		return 0;
	}
	int cntr = 0;
	for (int i = 0; i < lev-1; ++i){
		cntr += pow(8.0f, i);
	}

	return cntr;
}

void DrawNodeRec(float * inLowerC, 
	int lev, 
	int maxLev,
	float cellW,
	int ndx,
	std::vector<unsigned int> & inArray){

	glVertex3f(inLowerC[0], inLowerC[1], inLowerC[2]);
	glVertex3f(inLowerC[0] + cellW, inLowerC[1], inLowerC[2]);
	glVertex3f(inLowerC[0] + cellW, inLowerC[1] + cellW, inLowerC[2]);
	glVertex3f(inLowerC[0], inLowerC[1]+cellW, inLowerC[2]);

	// Find the children 
	int levSNdx = GetSNdx(lev);
	int sCndx = (levSNdx + pow(8.0f, lev-1) + 8*(ndx-levSNdx));
	cellW *= 0.5f;

	// Draw Children
	if (lev <= maxLev) {

		for (int c = 0; c < 8; ++c){
			int cNdx = sCndx+c;
			if (inArray[ndx])
			{
				float tmpLowerC[3] = {
					inLowerC[0] + cellW* ( c & 0x01),
					inLowerC[1] + cellW*( (c & 0x02) >> 1 ),
					0//inLowerC[2] + cellW*( (c & 0x04) >> 2 )
				};

				DrawNodeRec(tmpLowerC, lev+1, maxLev, cellW, cNdx, inArray);
			}
		}
	}

	return;
}

void DrawOctree(){
	if (testOctree.size() == 0){
		return;
	}

	// XXX Do this because reverse order

	// Returns number of levels - the actual root.
	unsigned int oNumLevs = cudaSim->GetOctreeNumLevs();

	int currLev = oNumLevs;
	int numCurrLev = std::pow(8.0f, float(currLev-1));
	int cntr = numCurrLev-1;

	float baseW = GetOctreeCellW(1, oNumLevs);
	float lowerC[3] = {baseW, baseW, baseW};

	glPolygonMode(GL_FRONT, GL_LINE);

	glBegin(GL_QUADS);

	
	// XXX Do this because octree is in reverse order
	DrawNodeRec(lowerC, 1, oNumLevs, -baseW, 0, testOctree);

	glPolygonMode(GL_FRONT, GL_FILL);

	glEnd();
}

void DrawForces() {
	if (testForces.size() == 0){
		return;
	}
	glColor3f(1.0, 0.0, 0.0);

	glBegin(GL_LINES);
	for (int i = 0; i < testParticles.size(); ++i){
		glVertex3fv(&testParticles[i].pos[0]);
		glVertex3f(testParticles[i].pos[0] + testForces[i*3],
			testParticles[i].pos[1] + testForces[i*3+1],
			testParticles[i].pos[2] + testForces[i*3+2]);
	}
	glEnd();
}

void DrawPress() {
	glBegin(GL_POINTS);
	for (int i = 0; i < testPress.size(); ++i){
		glColor3f(testPress[i], 0, -1*testPress[i]);
		glVertex3fv(&testParticles[i].pos[0]);
	}
	glEnd();
}

void DrawPartsBasedOnOctree() {
	glPointSize(3.0f);
	glColor3f(0.0,1.0,0.0);
	int otLev = cudaSim->GetOctreeNumLevs();
	glBegin(GL_POINTS);

	for (int i = 0; i < testParticles.size(); ++i){
		int cLev = otLev - testParticles[i].otLev;
		glColor3f(
			cLev == 0 ? 1.0 : 0.0,
			cLev == 1 ? 1.0 : 0.0, 
			cLev == 2 ? 1.0 : 0.0 );

			glVertex3f(testParticles[i].pos.x, testParticles[i].pos.y, testParticles[i].pos.z);
		
	}
	glEnd();
}

//Display
//This function will be called as fast as possible, unlike the Update func.
//Only GL display code should be put in this section.  The screen
//is automatically cleared to white in the InitFrames() func, and the 
//glfwSwapBuffers() is called by the default EndFrames() function, so you only
//have to worry about actual drawing here.
void Display() {

	glMatrixMode(GL_PROJECTION);
	glLoadIdentity();
	glOrtho(GRID_MIN_PT-1, GRID_MAX_PT+5, GRID_MIN_PT-1, GRID_MAX_PT+5, 0.01, 10 );

	glMatrixMode(GL_MODELVIEW);
	glLoadIdentity();

	glTranslatef(0,0,-1);

	
	glColor3f(1.0, 0.0, 0.0);
	
	// Draw Grid
	//DrawGrid();
	
	/*glColor3f(0.0, 1.0, 0.0);
	DrawPoints();*/

	DrawPartsBasedOnOctree();

	glColor3f(0.0,0.0,0.0);
	DrawOctree();

	glColor3f(1.0, 0.0, 0.0);
	//// Draw Forces
	//DrawForces();

	//DrawPress();

}

void DoPause() {
	sm.Pause();
}

void DoEmit() {

	// Create particles.
	CudaParticle p;
	float interval = SUPPORT_RAD*0.5;


	std::vector<CudaParticle> pList;

	for (float y = GRID_MIN_PT + 6; y <= 0.8*(GRID_MAX_PT-1);  y+= interval){
		for (float x = GRID_MIN_PT+SUPPORT_RAD; x <= 0.8*(GRID_MAX_PT-1);  x+= interval){
	
			p.pos.x = x; p.pos.y = y; p.pos.z = 0;
			p.vel = glm::vec3(0,0,0);

			pList.push_back(p);
		}
	}

	cudaSim->AddParticles(pList);
	cudaSim->SetMass(particleMass);

	printf("Generated %d particles. \n", pList.size());
	printf("Particles total: %d\n", cudaSim->GetNumParticles());
}



//Update
//This function will be called every timestep (ts).  The timestep can be
//set via the SetTimeStep method in SimulationManager. Put all non-drawing code
//here.
void Update(float ts) {
	//XXX
	//All update code goes here
	//...
	// Set
	IntegrateParticlesCUDA();
	//std::vector<int> testGridEnd, testGridNdx;

	cudaSim->GetGridEnd( testGridEnd );
	cudaSim->GetGridNdx( testGridNdx );
	cudaSim->GetParticles( testParticles);
	//cudaSim->GetForces( testForces );
	//cudaSim->GetPress(testPress);

	// REVERSE because octree is in reverse order...
	// XXX
	cudaSim->GetOctreeEoF(testOctree);
	std::reverse(testOctree.begin(), testOctree.end());

	// XXX Test to make sure no overlapping regions
	//for (int i = 0; i < testGridEnd.size(); ++i){
	//	for (int j = 0; j < testGridNdx.size(); ++j){
	//		if (i == j){
	//			continue;}

	//		if (testGridNdx[j] < testGridEnd[i] &&
	//			testGridNdx[j] >= testGridNdx[i]){
	//				printf("Error between cell %d and %d\n start: %d end: %d , start: %d end: %d\n", i, j, testGridNdx[i], testGridEnd[i], testGridNdx[j], testGridEnd[j]);
	//		}
	//	}
	//}

}


void LoadResources(){
	
	sm.SetTimeStep(TIME_STEP);
	
	float scellw = 2*SUPPORT_RAD;
	float snumcells = std::ceil((GRID_MAX_PT - GRID_MIN_PT)/(scellw));

	glm::vec3 cellW = glm::vec3(scellw, scellw, scellw);
	glm::ivec3 numCells = glm::ivec3(snumcells, snumcells, snumcells);


	// Create cuda simulator
	cudaSim = new CudaSPH(glm::vec3(GRID_MIN_PT, GRID_MIN_PT, GRID_MIN_PT),
		numCells, cellW);
	cudaSim->SetSPHParams(
		TIME_STEP,
		GAS_CONSTANT_K,
		VISCOSITY_CONSTANT, 
		GAMMA,
		SUPPORT_RAD);

	////Set up glew

	//GLenum err = glewInit();
	//if (GLEW_OK != err){
	//	fprintf(stderr, "Problem initializing GLEW: %s", glewGetErrorString(err) );
	//}
	//fprintf(stdout, "Status: Using GLEW %s\n", glewGetString(GLEW_VERSION) );
 //   if (!glewIsSupported("GL_VERSION_2_0 GL_ARB_pixel_buffer_object")) {
 //       printf("Required OpenGL extensions missing.");
	//}


}

int main (int argc, char ** argv ){


	sm.SetCallbackDisplay(Display);
	sm.SetCallbackUpdate(Update);
	sm.SetCallbackLoadResources(LoadResources);

	sm.AddKeyCallback('A', DoEmit);
	sm.AddKeyCallback('P', DoPause);

	//XXX

	//Start simulation.
	sm.Run();

	return 0;
}


void IntegrateParticlesCUDA() {

	//In case these change.  None of them should, but they can.
	//cudaSim.SetPhysParams( 
	//	GAS_CONSTANT_K,
	//	VISCOSITY_CONSTANT, 
	//	GAMMA,
	//	SUPPORT_RAD, 
	//	P0);

	cudaSim->Update();

}
